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ROSENBROCK-NYSTROM  INTEGRATORS  FOR  SSODE  OF 
MULTIBODY  DYNAMICS  * * * § 

A.  SANDTjt,  D.  NEGRUT1,  E.  J.  HAUG  §,  F.  A.  POTRA^,  AND  C.  SANDU  H 

Abstract.  When  performing  dynamic  analysis  of  a  constrained  mechanical  system,  a  set  of 
index  3  Differential- Algebraic  Equations  (DAE)  describes  the  time  evolution  of  the  system  model. 
The  paper  presents  a  state-space  based  method  for  the  numerical  solution  of  the  resulting  DAE.  A 
subset  of  so  called  independent  generalized  coordinates,  equal  in  number  to  the  number  of  degrees  of 
freedom  of  the  mechanical  system,  is  used  to  express  the  time  evolution  of  the  mechanical  system.  The 
second  order  state-space  ordinary  differential  equations  (SSODE)  that  describe  the  time  variation 
of  independent  coordinates  are  numerically  integrated  using  a  Rosenbrock  type  formula.  For  stiff 
mechanical  systems,  the  proposed  algorithm  is  shown  to  significantly  reduce  simulation  times  when 
compared  to  state  of  the  art  existent  algorithms.  The  better  efficiency  is  due  to  the  use  of  an  L-stable 
integrator  and  a  rigorous  and  general  approach  to  providing  analytical  derivatives  required  by  it. 

Key  words.  Multibody  dynamics,  differential-algebraic  equations,  state  space  form,  Rosenbrock 
methods. 

AMS  subject  classifications.  65L06 


1.  Introduction.  In  this  paper,  the  state  of  a  multibody  system  at  the  position 
level  is  represented  by  an  array  q  =  [qi. ... .  qn]T  of  generalized  coordinates.  The  ve¬ 
locity  of  the  system  is  described  by  the  array  of  generalized  velocities  q  =  [c/i , . . . ,  q„]T . 
Given  the  quantities  q  and  q,  the  position  and  velocity  of  each  body  in  the  system  is 
uniquely  determined. 

There  is  a  multitude  of  ways  in  which  the  set  of  generalized  coordinates  and  velocities 
can  be  selected  [5,  9,  6].  The  generalized  coordinates  used  in  this  paper  are  Cartesian 
coordinates  for  position  and  Euler  parameters  for  orientation  of  body  centroidal  refer¬ 
ence  frames.  Thus,  for  each  body  i  the  position  of  the  body  is  described  by  the  vector 
Pi  =  [xi,yi,Zi]T ,  while  the  orientation  is  given  by  the  array  of  Euler  parameters  [9], 
=  [e*o,  e*i,  e,2,  e,3]T.  Consequently,  for  a  mechanical  system  containing  nb  bodies, 

(1)  q=[pf  eT  ■■■  Pub  enb  ]T  GR7"6- 

When  compared  with  the  alternative  of  using  a  set  of  relative  generalized  coordinates, 
the  coordinates  considered  are  convenient  because  of  the  rather  complex  formalism 
employed  to  obtain  the  Jacobian  information  required  for  implicit  integration.  Note 
that  since  the  algorithm  proposed  in  this  paper  is  based  on  a  Rosenbrock  formula,  it 
becomes  essential  to  provide  accurate  integration  Jacobians  [7]. 

In  any  constrained  mechanical  system,  joints  connecting  bodies  restrict  their  rel¬ 
ative  motion  and  impose  constraints  on  the  generalized  coordinates.  To  simplify  the 
presentation,  only  holonomic  and  scleronomic  constraints  are  considered.  Kinematic 
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constraints  are  then  formulated  as  algebraic  expressions  involving  generalized  coordi¬ 
nates, 

(2a)  $(q)=[$i(q)  ...  $TO(q)  ]T  =  0 

where  m  is  the  total  number  of  constraint  equations  that  must  be  satisfied  by  the 
generalized  coordinates  throughout  the  simulation.  It  is  assumed  here  that  the  m 
constraint  equations  are  independent.  The  number  of  degrees  of  freedom  ndof  is 
thus  the  difference  between  the  number  of  generalized  coordinates  and  the  number  of 
constraints  ndof  =  n  —  m. 

Differentiating  Eq.(2a)  with  respect  to  time  leads  to  the  velocity  kinematic  constraint 
equation 

(2b)  $q(q)  q  =  0  , 

where  the  over  dot  denotes  differentiation  with  respect  to  time  and  the  subscript 
denotes  partial  differentiation,  3>q  =  d  ($i  ■  ■  ■  $m)  /d  (qi  ■  ■  ■  qn).  The  acceleration 
kinematic  constraint  equations  are  obtained  by  differentiating  Eq.(2b)  with  respect 
to  time, 

(2c)  $q(q)q  =  -(^q(q)q)qq  =  r(q,q)  . 

Equations  (2a)-(2c)  characterize  the  admissible  motion  of  the  mechanical  system. 

The  state  of  the  mechanical  system  changes  in  time  under  the  effect  of  applied 
forces.  The  time  evolution  of  the  system  is  governed  by  the  Lagrange  multiplier  form 
of  the  constrained  equations  of  motion  [9], 

(2d)  M(q)  q  +  $q  (q)  A  =  QA (q,  q,  t) 

where  M(q)  G  R"x"  is  the  symmetric  system  mass  matrix,  A  €  Rm  is  the  array  of 
Lagrange  multipliers  that  account  for  workless  constraint  forces,  and  Q^(q,  q,  t)  G  R” 
represents  the  generalized  applied  force  that  may  depend  on  the  generalized  coordi¬ 
nates,  their  first  time  derivatives,  and  time. 

Equations  (2a)-(2d)  comprise  a  system  of  differential-algebraic  equations  (DAE). 
It  is  known  that  differential-algebraic  equations  are  not  ordinary  differential  equations 
[13].  Analytical  solutions  of  Eqs.(2a)  and  (2d)  automatically  satisfy  Eqs.(2b)  and  (2c), 
but  this  is  no  longer  true  for  numerical  solutions.  In  general,  the  task  of  obtaining 
a  numerical  solution  of  the  DAE  of  Eqs.(2a)-(2d)  is  substantially  more  difficult  and 
prone  to  intense  numerical  computation  than  that  of  solving  ordinary  differential 
equations  (ODE) .  For  a  review  of  the  literature  on  numerical  integration  methods  for 
solution  of  the  DAE  of  multibody  dynamics  the  reader  is  referred  to  [14,  2]. 
Equations  (2c)-(2d)  are  expressed  in  matrix  form  as 

Qyl(q,  q,t) 

,r(q!q) 

Equations  (2a),  (2b),  and  (3)  must  be  satisfied  by  the  numerical  solution  to  be 
constructed.  This  set  of  index  3  DAE  [7]  is  first  reduced  to  a  set  of  state-space  ordinary 
differential  equations  (SSODE),  following  the  approach  proposed  by  [19].  First  an 
independent  subset  of  the  generalized  coordinates  q  is  determined.  The  partitioning  of 


M(q)  *q(q)J 

q 

.  *q(q)  0 
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the  generalized  coordinate  vector  q  e  R"  into  independent  and  dependent  coordinate 
vectors  v  e  R nd°f  and  u  €  R n-ndof , 

(4)  v[»]  =  q  [u(i)]  ,  1  <i<  ndof  ,  and  u[j]  =  q  [//(?)]  ,  1  <  3  <  n  ~  ndof  • 

is  done  such  that  the  sub-Jacobian  of  the  constraints  with  respect  to  u  is  nonsingular 

(5)  det  ($u(q))  ±  0  . 

Such  a  partition  can  be  found  starting  with  a  set  of  consistent  generalized  coordinates 
qo;  i.e.,  which  satisfy  Eq.(2a).  The  constraint  Jacobian  matrix  ‘I>q  is  evaluated  and 
numerically  factored,  using  the  Gauss- Jordan  algorithm  with  full  pivoting  [3], 

(6)  $q(qo)  ( Gauss  -  Jordan )  H-  [4>u(q0)|3>v(qo)] 

Column  pivoting  during  Gauss- Jordan  factorization  is  done  such  that  Eq.(5)  holds 
at  all  times.  Likewise,  the  functions  v  and  /i  introduced  in  Eq.(4)  are  defined  as  a 
byproduct  of  the  Gauss-Jordan  factorization.  Thus,  if  v  :  {1,  2, . .  i  ,ndof }  — >  Smdep 
and  j i  .  {1,  2, . . . ,  m  )  )■  Sdep  the  sets  Sijidep  <^nd  Sdep  such  that  Sindep  U  Sdep 
{1,2, ...  ,n}  and  Sindep  H  Sdep  =  0-  This  generalized  coordinate  partitioning  strategy 
is  possible  as  long  as  the  constraint  equations  of  Eq.(2a)  are  independent  [9];  i.e.,  as 
long  as  the  constraint  Jacobian  4>q  has  full  row  rank. 

Based  on  this  partitioning,  Eqs.(2a)-(2c)  can  be  rewritten  in  the  associated  par¬ 
titioned  form  [9] 

(7a)  Mvv(u,  v)v  +  Mvu(u,  v)ii  +  ^  (u,  v)  A  =  Qv(u,  v,  u,  v) 

(7b)  Muv(u,  v) v  +  Muu(u,  v)ii  +  $J(u,  v)A  =  Qu(u,  v,  u,  v) 

(7c)  $(u,  v)  =  0 

(7d)  4>u(u,v)u  +  $v(u,v)v  =  0 

(7e)  $u(u,  v)ii  +  $v(u,  v)v  =  r(u,  v,  u,  v) 

The  partitioning  of  Eqs.(7a)-(7e)  is  induced  by  the  partitioning  of  the  generalized 
coordinates  in  Eq.(4).  For  example,  Mvu[i,  j]  =  M [n(i) ,  n(j)],  for  1  <  i  <  ndof,  1  < 
j  <  n  —  ndof,  while  Qu[j]  =  Q[p(j)],  for  1  <  j  <  n  —  ndof.  Likewise,  a  partial  with 
respect  to  the  dependent  set  of  coordinates  u  is  obtained  by  gathering  the  columns 
yu(l)  through  /i(n  —  ndof)  of  the  derivative  with  respect  to  the  generalized  coordi¬ 
nates  q.  The  remaining  columns  provide  the  partial  with  respect  to  the  independent 
coordinates  v. 

The  condition  of  Eq.(5)  and  the  implicit  function  theorem  [4]  guarantee  that 
Eq.(7c)  can  be  solved  for  u  as  a  function  of  v, 


(8)  u  =  g(v) 

where  the  function  g(v)  has  as  many  continuous  derivatives  as  does  the  constraint 
function  4>(q).  For  all  but  the  most  simple  mechanical  systems,  an  analytical  ex¬ 
pression  for  the  function  g(v)  can  not  be  determined.  However,  for  any  consistent 
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configuration  qo  =  (uo,vo),  a  value  ii  can  be  found  for  each  v  in  a  small  enough 
neighborhood  of  v0.  The  value  u  is  computed  by  keeping  v  =  v  in  Eq.(7c)  constant 
and  solving  for  u  =  u.  Because  of  the  form  the  joint  constraint  equations  assume 
when  used  in  conjunction  with  a  Cartesian  representation,  u  is  always  the  solution  of 
a  system  of  non-linear  equations. 

The  system  of  DAE  in  Eqs.(7a),  (7b),  and  (7e)  is  reduced  to  an  SSODE,  through 
a  sequence  of  steps  that  use  information  provided  by  Eqs.(7c)  and  (7d).  First,  since 
the  coefficient  matrix  of  ii  in  Eq.(7d)  is  nonsingular,  u  can  be  determined  as  a  function 
of  v  and  v,  where  Eq.(8)  is  used  to  eliminate  explicit  dependence  on  u.  Next,  Eq.(7e) 
uniquely  determines  ii  as  a  function  of  v,  v,  and  v,  where  results  from  Eqs.(7d)  and 
(8)  are  substituted.  Since  the  coefficient  matrix  of  A  in  Eq.(7b)  is  nonsingular,  A  can 
be  determined  uniquely  as  a  function  of  v,  v,  and  v,  using  previously  derived  results. 
Finally,  each  of  the  preceding  results  is  substituted  into  Eq.(7a)  to  obtain  the  SSODE 
in  the  independent  generalized  coordinates  v  [19], 


(9) 

M(v)  v  =  Q(t,v,v) 

where 

(10a) 

M  = 

Mvv  -  Mvu«f>u1<f>v  -  [Muv  - 

(10b) 

Q  = 

Qv-Mvu$-1r-^$-T  [Qu-Muu$-1t]  . 

The  SSODE  (9)  is  well  defined,  due  to  the  following  property  [9]. 

Lemma  1.1.  For  anyv  €  R,nd°f ,  the  matrix  M  (v)  ofEq.(lOa)  is  positive  definite. 

Proof.  Given  v  €  Hnd°f ,  the  position  configuration  of  the  mechanical  system  is 
uniquely  determined  with  all  remaining  dependent  coordinates  provided  by  Eq.(8). 
For  any  v  £  Hndofi  define  u  =  —  $“1(u,  v)$v(u,  v)v.  Then  the  pair  v,  ii  determines 
a  consistent  set  of  generalized  velocities,  as  it  satisfies  the  velocity  kinematic  constraint 
equations  of  Eq.(7d).  If  the  generalized  velocity  corresponding  to  the  pair  v  ^  0,  ii 
is  denoted  by  q,  the  kinetic  energy  K  =  l/2qTMq  >  0.  Since  qTMq  =  vTMv,  it 
follows  that  vtMv  >  0.  □ 

2.  Rosenbrock  integration  formulas  for  second  order  systems.  For  the 

Initial  Value  Problem  (IVP),  y'  =  y(t0)  =  y0,  an  s-stage  Rosenbrock  method 

is  defined  as  [7] 

(11a)  y-n+l  —  Un  X/i=l  ^iki  j 

(lib)  hi  =  hf  (tn  +  atih,  yn  +  Yj)=\  aijkj )  +  lih2  %  (tn,  yn)  +  h.J  Y,)=i  Hjkj  ■ 

where  the  number  of  stages  s  and  the  coefficients  6*,  a,j,  and  7^  are  chosen  to  obtain 
a  desired  order  of  consistency  and  stability,  J  =  fy(tn.  yn).  oti  =  X)j=i  °-ij-  and 
7 i  =  J2lj=  1  7 ij-  F°r  reasons  of  computational  efficiency  the  coefficients  7^  are  identical 
for  all  stages;  i.e.,  7,,  =  7  for  all*  =  1, . ... ,  s.  Note  that  formally  an  =  0,  1  <  *  <  s. 
For  the  purpose  of  error  control  in  the  the  generic  Rosenbrock  method  a  second 
approximation  of  the  solution  at  the  current  time  step  is  used  to  produce  an  estimate 
of  the  local  error.  This  second  approximation  yn+i  is  usually  of  lower  order  and  it 
recycles  the  stage  values  fcj  of  Eq.(llb),  but  this  time  employing  a  different  set  of 
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coefficients  bj, 

(12)  Vn+ 1  —  Dn  "t"  ^  (  biki 

i=  1 

The  approximation  \yn+i  —  y„+i  of  the  local  error  depends  on  the  size  of  the 
integration  step-size,  and  the  latter  is  increased  or  decreased  to  keep  the  local  error 
right  below  a  user  prescribed  absolute  and/or  relative  tolerance.  Thus,  for  the  vector 
case  y  e  R”,  at  time  step  n  +  1  the  error  in  component  i  is  kept  smaller  than  a 
composite  error  tolerance  sc* 

(!3)  IVn+i  ~  vh+  ll  <  sci,  sci  =  Atoli+m&x(\yln\,  \yln+l\)  ■  Rtoli 

where  Atoli  and  Rtoli  are  the  user  prescribed  absolute  and  relative  integration  toler¬ 
ances  for  component  i.  The  value 


(14) 


err  = 


}_  (Vn+1  Vn+ 1)2 

m  sc? 

i=i  1 


1/2 


is  considered  as  a  measure  of  local  error.  If  the  order  of  the  proper  and  embedded 
formulas  used  is  p  and  p  respectively,  asymptotically  err  ss  Chq+1,  where  C  is  a 
constant  dependening  on  the  choice  of  formulas  and  q  =  min(p,p).  Optimally,  err  =  1 
and  therefore  1  fti  Ch\ ]p^ .  The  optimal  step-size  is  computed  then  as 

/  1  \  5TT 

(15)  hopt  =  h[ - 

\err  ) 


A  safety  factor  fac  multiplies  hopt  to  decrease  the  chance  of  a  costly  rejected  step-size, 
which  happens  whenever  err  >  1.  Further,  the  step-size  is  not  allowed  to  increase  or 
decrease  too  fast.  This  is  achieved  by  two  control  parameters  facmin  and  facmax, 

(16)  hneu ,  =  Zimin  ^facmax,  max  (^facmin,  fac  ■  (1  /err)1/,(9+1')^ 

To  apply  a  generic  Rosenbrock  formula  for  the  solution  of  the  SSODE  of  Multi¬ 
body  Dynamics  of  Eq.(9),  first  notice  that  since  M  is  positive  definite,  by  multiplying 
from  the  left  with  M-1,  the  second  order  SSODE  of  Eq.(9)  theoretically  can  be  locally 
reduced  to  the  form 


(17a)  y"  =  f(t,  y,  y') 

Note  that  for  the  purpose  of  introducing  the  Rosenbrock-Nystrom  approach  and  with¬ 
out  any  loss  of  generality  in  the  formula  above  the  vector  v  was  replaced  with  a  scalar 
quantity  y.  As  the  interest  is  only  in  determining  the  coefficients  of  the  Rosenbrock- 
Nystrom  method,  the  fact  that  this  formula  is  used  to  find  a  scalar  or  vector  numerical 
solution  of  an  ODE  problem  is  irrelevant. 

The  previous  second  order  system  of  ODE  is  transformed  into  a  standard  first 
order  ODE  problem, 


y 

i 

y' 

.  y' . 

_  f(t,y,y')  _ 

(17b) 
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Applying  the  generic  method  of  Eqs.(lla-  lib)  to  this  first  order  ODE  system  yields 


where 


y  +Eb* 


y'n  +  aijlj 

f  {tn  "h  atih,  yn  +  E,-=i  aijkj,  yn  +  Ey= 1  aij^j) 


,2  r  o 

+  ^ih  SlU  u  y> 

L  dt  ^n’  yn. 


0  I 


(19)  Ji  =  ^{tn,yn,y'n)  and  J2  =  ^(tn,yn,y'n)  ■ 

In  order  to  obtain  a  numerical  method  to  directly  integrate  Eq.(17a),  the  “y-stages” 
ki  are  eliminated  to  express  the  formula  only  in  terms  of  the  “y'-stages”  E  Defining 
Bij  =  otij  +  7 ij,  the  first  row  of  (18b)  is 


h  =  hy’n  +  h  ^  A 


ij  1  —  1,  •  ■  ■  ;  S 


In  the  second  row  of  Eq.(18b),  the  sum  Ej= 1  aijkj  comes  in  as  the  second  argument 

of  /(■,■,•)•  Defining  Sij  =  E™=j  and  using  Eq.(20)  and  the  summation 

interchange  procedure,  this  sum  is  expressed  in  terms  of  Ij  as 


El  aijkj  —  aij  f  ^ Vn  +  ^  Prr 

j=  1  j=l  V  m  l 

2—1  j 

=  hoiiy'n  +  h  EE 

j= 1  m=l 
i— 1  i— 1 

=  hatiy'n  +  h  E  E°«& 

m=  1  j=m 


—  hotiUn  +  ^  ^  ^  ^ij^j 
j=1 

With  the  above  substitution,  the  second  row  of  Eq.(18b)  becomes 

/  i- 1  i— 1 

(21)  £i  —  hf  I  tn  H-  Qiih)  yn  H-  hociyn  -b  /i  ^  ^  Vn  ^  ^ 


+7^2-7^(*n,2/n,2/n) 

+/1J1  7*  j  + hJ2 

j= 1  j=l 
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Defining  9ij  =  J2m=j  limPmj  and  substituting  the  whole  sum  in  k's  by  a  sum  in 
Vs 

i  i 

y  ]  lij  kj  =  hliDn  +  h  y'  9jj£j 

3= 1  3  3. 

Eq.(21)  becomes  the  stage  relation 

(i-l  i- 1  \ 

in  +  cxih ,  yn  +  ha%yn  +  h  ^  (  Sij£j,  yn  +  ^  (  aij^j  J 

3= i  r  i  / 

+7i^2  (j^(tn,yn,y'n)  +  JiyCj 

i  i 

-\-ti~  j  i  +  hj-2  y  (  7 jjij 

3=  1  Jf=l 

Z,From  a  computational  point  of  view  Eq.(22)  has  the  following  interpretation:  at 
stage  *  (1  <  i  <  s)  the  quantity  V  is  to  be  found  as  the  solution  of  a  linear  system 

SiU  =  RES 

where  Si  =  I—  h-juJ 2  —  h?9uJ\.  Since  an  =  0,  1  <  *  <  s,  it  follows  that  9  a  =  7  a  fin  = 
7 n{pm  +7 a)  =  72.  Thus,  the  linear  systems  to  be  solved  at  each  stage  have  the  same 
matrix, 

Si  =  S  =  I  —  /17J2  —  h2y2  Ji 

Substituting  k's  by  Vs  in  Eq.(18a)  and  denoting  yn  =  Y^]=i  bjPji  leads  to 
(23a)  Dn+ 1  —  Un  +  hy-n  "b  h  y  \  /J-i^i 

i—1 

(23b)  yn+l  =  Vn  ~b  biV 

i=  1 

Using  matrix  notation  for  the  coefficients  (e.g.,  (a,j)  is  the  matrix  whose  entries  are 
the  a-coefhcients  of  the  method)  <5, 9,  and  /1  are  expressed  as 

(fiij)  =  ( aij )  ’  (Pij)  J  (Qij)  =  (7*j)  ’  (Pij)  J  (/^i)  =  (&i)  ’  (/?ij)  ■ 

To  summarize,  the  following  linearly  implicit  method  for  the  second  order  system 
(17a)  is  defined: 


(24a) 


9n+ 1  —  y?i  +  ^ZZn  T  h  y  \  i  f  i 

i=  1 

ZZn+l  =  Vn  +  W* 
i=l 


(24b) 
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i— 1 

Yi  —  Un  H-  haiyn  H-  h  ^  ^  ^ij^j 
j=  i 

i—  1 

y/  =  +  XI  q7<j 

j=l 

U  =  hf  (tn  +  aih,Yi,Yf) 

+7 ih2  (j^(tn,yn,y'n)  +  JiyCj 

i  i 

+ft2  Ji  y  '  Oijtj  +  hj‘2  5^7 ijlj 

3=  i  j=i 

It  can  be  seen  in  Eq.(22)  that  matrix- vector  multiplications  are  needed.  Because 
of  the  presence  of  both  Ji  and  J2,  the  classical  transformation  removes  only  the 
multiplications  with  one  of  the  J’s.  Substituting  Zi  =  J2)=i  7 ij^j  into  the  method, 
Eqs.(24a-24e)  leads  to  the  following. 

Theorem  2.1.  Let  (a^),  (7 ij),  (bj),  and  (bj),  be  the  coefficients  of  an  s-stage 
embedded  Rosenbrock  method  given  by  Eqs.(lla-llb).  The  associated  Rosenbrock- 
Nystrom  method  is  defined  as 

(25a)  yn+1  =  yn  +  hy'n  +  h  ^  fijZj  ,  yn+1  =  yn  +  hy'n  +  h  ^  ffizj 

i—1  i=  1 

(25b)  y'n+ 1  =  y'n+^2  mizi  ,  y'n+l  =y'n  +  Yl  ™iZi 

i= 1  i= 1 

i—1 

(25c)  Yi  —  yn  +  ha,iyn  H-  ^  @ij%j 

3= 1 

i—1 

(25d)  Yi  =  yn  +  aijzj 

3= 1 

(25e)  S  =  I  -  h~iJ2  -  h272  Ji 

(25f)  S-z*  =  hjf  (tn  +  aih,Yi,Y-)  +  h?jjj  (j^(tn,yn,y'n)  +  JiyCj 

i—1  i—1 

+7  Y  Ciizi  +  h2^Jl  Y  Siizi 
3=1  3=  1 

where  the  new  coefficients  are 

( aij )  =  (Q*j)  ’  (7«j) 

(Cij)  =  7_1/-(7ij)_1 

(dij)  =  (7 *j)  +  (7*j)  '  (aij)  ’  (7*j) 

(@ij)  =  (aij)  "h  (Q*j)  ■  (7*j) 

(mj)  =  (&i)  •  (7ij)_1 

(mj)  =  (b^j  ■  (lij)-1 


(24c) 

(24d) 

(24e) 
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(/ti)  —  ( bi )  +  (&j)  •  (ctjj)  '  (7 ij) 

(/*»)  =  ’  (aij)  '  (7 ij) 

(7 i)  =  (1,  •■•,!)  '  (7ij)T 

(«i)  =  (1,  •••,!)  •  (a*j)T 

Proof.  By  direct  substitution.  □ 

Note  that  in  Eqs.(25a)  and  (25b),  values  for  a  second,  and  typically  lower  or¬ 
der,  approximation  is  provided  at  time  f„+i  for  both  the  solution  yn+\  and  its  first 
derivative  y'r l+1.  These  values  are  used  for  step-size  control,  as  indicated  in  Eqs.(12) 
through  (16). 

3.  Providing  analytical  integration  Jacobians.  A  successful  implementa¬ 
tion  of  the  Rosenbrock  family  of  formulas  introduced  above  depends  upon  the  ability 
to  provide  exact  derivative  information.  Equation  (19)  indicates  the  derivatives  that 
must  be  computed.  The  numerical  solution  of  a  dynamic  analysis  problem  carried  out 
in  the  proposed  framework  of  a  Rosenbrock  implicit  integrator  requires  exact  com¬ 
putation  of  the  derivatives  of  independent  acceleration  with  respect  to  independent 
positions  and  velocities, 

(26)  Ji  =  vv  ,  J2  =  vv  . 

To  obtain  these  derivatives,  first  differentiating  Eq.(7a)  with  respect  to  v  yields 

(27)  MvvJi  +  (Mvvv)v  +  (Mvvv)uiiv  +  Mvuiiv  +  (Mvuii)v  +  (Mvuii)uuv 

+  <AV  +  (0)v  +  (0)uUv  =  Q:+Q>v  +  Q>v 

The  partial  differential  notation  is  explained  in  the  Appendix.  The  quantities  uv, 
iiv,  iiv,  and  Av  are  obtained  by  taking  partials  with  respect  to  v  of  Eqs.(7c),  (7d), 
(7e),  and  (7b),  respectively.  This  is  an  exercise  in  chain  rule  differentiation  that  yields 
[10,  11] 

(28a)  uv  =  -$“1$v=H 

(28b)  uv  =  -*"1  [(*qq)v  +  ($qq)uH]  =  J 

(28c)  iiv  :  HJ 1  -f-  L 

(28d)  Av  =  -$-t[R+(Muv+MuuH)Ji] 

where 

(29)  L  =  S”1  [  [ru  -  (*qq)u]  H  +  rv  +  ruJ  -  (*qq)v]  , 

(30)  R  =  [($„A)U  +  (Muq)u  —  Q“]  H  —  Q“  —  QJJJ 

+  (^uA)  v  +  (Muq)v  +  MUUL  ,  with  Mu  =  [Muv,  Muu]  . 
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Substituting  the  expressions  for  uv,  iiv,  iiv,  and  Av  into  Eq.(28)  and  denoting 
Mv  =  [MVV,MVU]  yields 

(31)  MJX  =  Qv  +  qvh  +  QYJ 

-  [MVUL  +  HtR  +  «A)uH  +  «A)  v  +  (Mvq)v  +  (Mvq)uH] 

According  to  Lemma  1.1,  the  coefficient  matrix  in  this  multiple  right  side  linear  system 
is  positive  definite.  Therefore,  Eq.(32)  properly  defines  the  derivative  Ji. 

Computation  of  J2  =  vy  follows  the  steps  taken  for  the  computation  of  Ji .  Taking 
the  derivative  of  Eq.(7a)  with  respect  to  v  yields 

(32)  Mvv  J2  +  Mvuiiy  +  C  Ay  =  QYfiy  +  Q  Y 

All  derivatives  in  this  expression  are  available,  except  the  quantities  J2,  iiy,  iiy,  and 
Ay.  The  last  three  derivatives  are  obtained  by  taking  partial  derivatives  with  respect 
to  the  independent  velocity  v  of  Eqs.(7d),  (7e),  and  (7b).  By  repeatedly  applying  the 
chain  rule  of  differentiation  these  derivatives  are  obtained  as 

(33a)  iiy  =  H 

(33b)  iiy  =  N  +  HJ2 

(33c)  Ay  =  [QVH  +  QV  -  MUUN  -  (Muv  +  MUUH)  J2] 

where 

(34)  N  =  $-1(ruH  +  ry) 

Substituting  these  results  into  Eq.(32),  the  derivative  of  independent  accelerations 
with  respect  to  independent  velocities  is  obtained  as  the  solution  of  the  multiple  right 
side  system  of  linear  equations, 

(35)  MJ2  =  W  -  HtX 
where 

W  =  Q^H  +  Qy  -  MVUN  , 

X  =  -  (QVH  +  QV  -  MUUN)  . 

With  M  positive  definite,  Eq.(35)  properly  defines  the  derivative  J2. 

A  general  framework  is  provided  in  this  Section  to  analytically  express  the  inte¬ 
gration  Jacobian  required  by  the  Rosenbrock  family  of  integration  formulas.  Although 
rather  involved  in  form,  these  derivatives  are  obtained  in  a  straightforward  way.  Fur¬ 
thermore,  they  are  generic,  in  the  sense  that  they  apply  to  any  mechanical  system 
simulation.  It  remains  to  provide  all  the  ingredients  that  explicitly  or  implicitly  en¬ 
ter  the  right  side  of  Eqs.(32)  and  (35).  These  derivatives  change  according  to  the 
modeling  elements  used  to  represent  a  mechanical  system.  What  makes  the  approach 
viable  is  the  fact  that  even  these  derivatives  can  be  generated  in  a  completely  generic 
way.  This  is  solely  a  mechanical  system  modeling  task  that  hinges  upon  the  fact 
that,  in  multibody  dynamics,  all  modeling  elements  are  broken  down  into  primitives. 
Providing  required  derivative  information  for  these  primitives  in  Cartesian  coordi¬ 
nates  is  tractable.  Derivative  information  for  primitives  is  then  combined  to  produce 
derivatives  for  complex  modeling  entities.  To  illustrate  this,  consider  the  joints  used 
to  connect  bodies  in  a  mechanical  system  model.  The  vast  majority  of  them  can  be 
obtained  starting  from  four  simple  constraint  primitives  [9]: 
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4>rfl  Dot-1  constraint  primitive,  imposes  that  two  body-fixed  non-zero  vectors  a t 
and  a j  belonging  to  bodies  i  and  j  respectively,  should  be  perpendicular  at 
all  times;  i.e.,  (a^a^)  =  a^a^  =  0 

$d2  Dot-2  constraint  primitive,  imposes  that  one  body-fixed  vector  a*  and  a 
vector  dy  defined  by  two  body-fixed  points  Pi  and  Pj  should  be  perpendicular 
at  all  times;  i.e.,  <&d2  (aj,dy)  =  ajTdy  =  0 

4>s  Point  Constraint  Primitive,  imposes  that  two  body-fixed  points  Pj  and  Pj 
belonging  to  bodies  i  and  j  respectively,  should  coincide  at  all  times;  i.e., 
*s(Pi,Pj)  =  Pt=P, 

<f>dlst  Distance  Constraint  Primitive,  imposes  that  the  distance  between  two 
body-fixed  points  Pi  and  Pj  belonging  to  bodies  i  and  j  should  stay  constant 
and  equal  to  C  >  0  at  all  times;  i.e.,  (Pj,  Pj.  C )  =  dist  ( Pi ,  Pj)  =  C 
In  this  context,  a  universal  joint  that  allows  two  relative  degrees  of  freedom  between 
the  constrained  bodies  is  defined  by  requiring  that  a  Dot-1  and  a  Point  Constraint 
Primitive  be  simultaneously  satisfied  throughout  the  simulation.  Likewise,  a  spherical 
joint  that  allows  three  degrees  of  freedom  (rotational)  between  the  constrained  bodies 
is  simply  a  Point  Constraint  Primitive,  while  a  revolute  joint  is  the  assembly  of  two 
Dot-1  and  one  Point  Constraint  primitives. 

Analyzing  the  order  of  derivatives  used  to  compute  the  required  derivative  infor¬ 
mation  Ji  and  J2,  it  can  be  seen  that  the  highest  order  is  3,  and  it  appears  as  a  result 
of  taking  partials  of  the  right  side  of  the  acceleration  kinematic  constraint  equation. 
Consequently,  derivatives  of  the  four  constraint  primitives  introduced  above  should 
be  implemented  up  to  order  3.  Deriving  and  coding  expressions  for  all  derivatives 
for  the  modeling  primitives  up  to  order  3  is  a  one  time  effort.  For  details  about  how 
these  derivatives  are  obtained  for  constraint  primitive,  inertia  elements,  and  forces 
the  interested  reader  is  reffered  to  [16].  For  the  scope  of  the  present  paper,  it  suf¬ 
fices  to  assume  that  the  derivatives  required  to  analytically  compute  the  Rosenbrock 
integration  Jacobian  are  readily  available. 

4.  Proposed  algorithm.  For  multibody  dynamic  analysis  problems,  a  low  to 
medium  accuracy  method  with  very  good  stability  properties  is  desirable.  Formulas 
with  few  function  and  Jacobian  evaluations  are  favored,  since  obtaining  accelerations 
and  the  integration  Jacobian  are  costly  operations.  A  method  that  is  L-stable  allows 
for  robust  integration  of  very  stiff  problems,  which  enables  efficient  handling  of  bush¬ 
ing  elements  and  flexible  components  used  in  modeling  mechanical  systems.  In  this 
context  the  focus  is  on  a  L-stable  order  4  Rosenbrock-Nystrom  method  with  4  stages. 
This  allows  for  room  in  determining  a  second  set  of  coefficients  that  efficiently  provide 
an  order  3  embedded  formula  for  step-size  control.  Furthermore,  following  an  idea  of 
[7],  the  number  of  function  evaluations  for  the  4  stage  method  is  kept  to  3;  i.e.,  one 
function  evaluation  is  saved.  In  terms  of  function  evaluations,  this  makes  the  proposed 
Rosenbrock-Nystrom  method  competitive  with  the  trapezoidal  method,  whenever  the 
latter  requires  3  or  more  iterations  for  convergence.  Moreover,  the  trapezoidal  method 
is  of  order  2  and  only  weakly  A-stable. 

With  =  J2*jl\  Pij,  the  defining  coefficients  a, j,  7^,  and  6j  of  an  order  4  Rosen¬ 
brock  method  of  Eqs.(lla-  lib)  are  subject  to  the  following  order  conditions  [7]: 

(36a)  b\  +  62  +  b3  +  ^4  =  1 

(36b)  62/^2  +  b3f3'3  +  ^4^4  =  1/2  —  7 

(36c)  b2a  2  +  b3al  +  &4a2  =  1/3 

(36d)  b3/332/32  +  ^4  (/?42/?2  +  /?43 /?3 )  =  1/6  —  7  +  l2 
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(36e)  620^  +  630!  +  640:4  —  1/4 

(36f)  6303032/^2  +  6404  (042/^2  +  a43^3)  =  1/8  —  7/3 

(36g)  63/33202  +  64  (,04202  +  /343O3)  =  1/12  -  7/3 

(36h)  64/343/332/?2  =  !/24  ~  l/2  +  I.572  -  73 

The  stage  values  ki  of  the  order  4  method  are  recycled,  for  the  purpose  of  au¬ 
tomatic  step-size  control,  to  provide  an  embedded  formula  of  order  3  of  the  form 
iji  =  r/o  +  2i=i  6 ih.  The  order  conditions  for  the  order  3  algorithm  are  as  indicated 
as  Eqs.(36a-  36d).  These  conditions  lead  to  the  system 


'111  1 

'  h  - 

1 

0  /3'  /3g  ^4 

62 

1/2-7 

0  o|  a\  a\ 

63 

i/3 

0  0  P32P2  P42P2  +  /?43  /?3 

.  64  . 

1  /6  -  7  +  72 

If  the  coefficient  matrix  in  Eq.(37)  is  non-singular,  uniqueness  of  the  solution  of  this 
linear  system  implies  61  =6*.  To  prevent  this,  one  additional  condition  is  considered 
to  obtain  a  distinct  order  3  embedded  formula.  It  requires  the  coefficient  matrix  in 
Eq.(37)  to  be  singular,  which  results  in  the  condition 

(38)  $32  $2  (/^2^4  —  $ 4a2)  =  ($2a3  —  $3a2)i$^$'2  + 

The  number  of  coefficients  that  must  be  determined  is  17;  the  diagonal  coefficient  7, 
six  coefficients  7 six  coefficients  otij,  and  four  weights  6*.  The  number  of  conditions 
that  these  coefficients  have  to  satisfy  is  nine.  There  are  eight  degrees  of  freedom  in 
the  choice  of  coefficients  and  some  of  these  are  used  to  construct  a  method  with  one 
less  function  evaluation.  Thus,  if 


«4i  =  a  31 

(39)  «42  =  Q!32 

O43  =  0 


stage  4  of  the  algorithm  saves  one  function  evaluation.  Finally,  the  free  parameters  can 
be  determined  such  that  several  order  5  conditions  of  the  otherwise  order  4  proposed 
formula  are  satisfied.  When  the  conditions  of  Eq.(39)  hold,  one  of  the  nine  order  5 
conditions  associated  with  a  Rosenbrock  type  formula  leads  to 


(40) 


_  1/5  - 0:2/4 
a3  1/4-o2/3 


A  second  order  5  condition  is  satisfied  by  imposing  the  condition 


(41)  64/3430^(03  -  o2)  =  1/20  -  7/4  -  o2  (1/12  -  7/3) 

Next,  two  conditions  are  chosen  as 


(42) 


63  =  0 

o2  =  27 


to  make  the  task  of  finding  the  defining  coefficients  o,j,  7 y,  and  6j  more  tractable. 
Finally,  the  last  condition  regards  the  choice  of  the  diagonal  element  7.  The  value 
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of  this  parameter  determines  the  stability  properties  of  the  Rosenbrock  method.  In 
this  context,  the  diagonal  entry  of  the  Rosenbrock  formula  is  suggested  in  [7]  as 
7  =  0.57281606,  which  is  adopted  for  the  proposed  algorithm.  With  this,  there  is  a 
set  of  17  equations,  some  of  them  non-linear,  in  17  unknowns.  The  solution  of  this 
system,  accurate  up  to  25  digits,  is  provided  below,  along  with  the  coefficients  bi  of 
the  order  3  embedded  formula. 


|  7  =  0.57281606  | 

a2i  =  1.14563212 

Q3i  =  0.520920789130629029328516 

a32  =  0.134294186842504800149232 

a4i  =  0.520920789130629029328516 

a42  =  0.134294186842504800149232 

a  43  =  0.0 

721  =  2.341993127112013949170520 

731  =  -0.027333746543489836196505 

732  =  0.213811650836699689867472 

741  =  -0.259083837785510222112641 

742  =  -0.190595807732311751616358 

743  =  -0.228031035973133829477744 

bi  =  0.324534707891734513474196 

b2  =  0.049086544787523308684633 

63  =  0.0 

64  =  0.626378747320742177841171 

61  =  0.520920789130629029328516 

62  =  0.144549714665364599584681 

63  =  0.124559686414702049774897 

bi  =  0.209969809789304321311906 

Once  the  coefficients  of  the  underlying  Rosenbrock  formula  are  available,  the 
coefficients  of  the  Rosenbrock-Nystrom  formula  defined  in  Theorem  2.1  are  easily 
computed.  The  full  set  of  coefficients  for  the  order  4,  L-stable  formula  is  provided 
below. 


02i  =  1.14563212 

03i  =  0.789509162815638629626980 

032  =  0.134294186842504800149232 

04i  =  0.789509162815638629626980 

042  =  0.134294186842504800149232 

043  =  0.0 

021  =  0.20000000000000000000000 
a3i  =  1.86794814949823713234476 
a32  =  0.23444556851723885002322 
041  =  1.86794814949823713234476 
a42  =  0.23444556851723885002322 
0/43  —  0.0 

C21  =  -7.137649943349979830369260 

c3i  =  2.580923666509657714488050 

c32  =  0.651629887302032023387417 

c4i  =  -2.137115266506619116806370 

c42  =  -0.321469531339951070769241 

c43  =  -0.694966049282445225157329 

621  =  -1.196361007112013949170520 
d3i  =  1.470280254409780714633870 
S32  =  0.348105837679204490016704 
d4i  =  0.003765094355556165798974 
J42  =  -0.109762486758103255675398 
J43  =  -0.228031035973133829477744 

mi  =  2.255566228604565243728840 
m2  =  0.287055063194157607662630 
m3  =  0.435311963379983213402707 
rru  =  1.093507656403247803214820 

mi  =  2.068399160527583734258670 
m2  =  0.238681352067532797956493 
m3  =  0.363373345435391708261747 
rhi  =  0.366557127936155144309163 

pi  =  1.592750819409585342074900 
p2  =  0.195938266310250609693329 
p3  =  0.0 

IM  =  0.626378747320742177841171 

pi  =  1.434903971848209472627100 
p2  =  0.222978672588698369045153 
p3  =  0.124559686414702049774897 
p4  =  0.209969809789304321311906 

72  =  -1.769177067112013949170520 

73  =  0.759293964293209853670967 

74  =  -0.104894621490955803206743 

02  =  1.145632120 
a3  =  0.655214975973133829477748 
a4  =  0.655214975973133829477748 

It  should  be  recalled  that  any  Rosenbrock  type  formula  requires  an  exact  Jacobian 
for  the  numerical  solution  to  maintain  its  stability  and  accuracy  properties.  Sometimes 
this  might  be  a  very  challenging  requirement.  Consider  for  example  the  situation 
when  complex  tire  models  are  present  in  a  model,  or  for  a  general  purpose  solver  the 
case  when  user  defined  external  routines  are  employed  for  the  computation  of  active 
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forces  such  as  aerodynamic  forces.  Providing  an  exact  Jacobian  for  these  situations 
is  very  unlikely.  Verwer  et  al.  1997,  proposed  a  second  order  W-method  [8],  which 
is  a  Rosenbrock  type  method  in  the  sense  that  it  does  not  necessitate  the  solution 
of  a  non-linear  system,  but  which  does  not  require  an  exact  Jacobian.  The  defining 
coefficients  for  this  method  are  provided  in  the  table  below. 


j  7  =  1.70710678118650  I 

a\ 

= 

0.00000000000000 

71 

= 

1.70710678118650 

OL2 

= 

1.00000000000000 

72 

= 

-1.70710678118650 

«21 

= 

0.58578643762690 

C21 

= 

-1.17157287525380 

dn 

= 

1.70710678118650 

$22 

= 

1.70710678118650 

$21 

= 

-2.41421356237310 

6*21 

= 

1.00000000000000 

mi 

= 

0.87867965644040 

mi 

= 

1.17157287525380 

m2 

= 

0.29289321881340 

m2 

= 

0.58578643762690 

pi 

= 

0.79289321881340 

pi 

= 

0.58578643762690 

P2 

= 

0.50000000000000 

p  2 

= 

1.00000000000000 

This  paper  is  concerned  with  the  implementation  of  a  Rosenbrock-Nystrom  based 
method.  Algorithm  1  based  on  the  4  stage,  order  4  L-stable  Rosenbrock  formula 
introduced  is  presented  below.  The  W-method  can  be  similarly  implemented  by  re¬ 
placing  the  corresponding  coefficients  of  the  Rosenbrock-Nystrom  formula  with  the 
coefficients  provided  in  the  previous  table.  Details  of  this  implementation  and  the 
performance  of  such  a  method  are  not  the  presented  here. 

Algorithm  1 

1 .  Initialize  Simulation 

2.  Set  Integration  Tolerance 

3.  While  (time  <  time-end)  do 

4.  Set  Macro-step 

5.  Get  Integration  Jacobian 

6.  Factor  Integration  Jacobian 

7.  Get  Time  Derivative 

8.  Resolve  Stage  1 

9.  Resolve  Stage  2 

10.  Resolve  Stage  3 

11  Resolve  Stage  4 

12.  Get  Solution.  Check  Accuracy.  Determine  New  Step-size 

13.  Recover  Dependent  Generalized  Coordinates 

14.  Check  Partition 

15.  End  do 


Step  1  initializes  the  simulation.  Based  on  user  provided  values  at  time  to,  a 
consistent  set  of  initial  conditions  (uo,  v0,  iio,  v0);  i.e.,  satisfying  Eqs.(7c)  and  (7d), 
is  determined  and  simulation  starting  and  ending  times  are  defined.  User  defined 
integration  tolerances  Atoli,  and  RtoU  of  Eq.(13)  are  read  in  and  set  during  Step  2. 
These  tolerances  are  used  to  control  error  in  both  independent  position  v  and  velocity 
v.  Step  4  backs  up  the  system  configuration  to  be  used  upon  a  rejected  time  step. 
During  Step  5,  the  integration  Jacobian  is  evaluated.  Since  obtaining  Jx  and  J2  is  a 
costly  operation,  Eqs.(32)  and  (35)  suggest  that  z,  is  more  efficiently  computed  if  the 
stage  linear  system  of  Eq.(25f)  is  replaced  with  an  equivalent  one  obtained  by  formally 
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multiplying  the  original  linear  system  with  the  positive  definite  matrix  (1  /7)M.  The 
new  linear  system  assumes  the  form 

(43)  IIz,  =  r  i 
where,  with  M  and  Q  defined  in  Eqs.(lOa)  and  (10b), 

(44)  II=^M'S=^M-  -  MJ2  —  h2MJi 

/y^  ryZ  /y 

h  (  dv 

(45)  r i  =  -Q  (tn  +  otih,  Vj,  +  h2^-  NVl-^(f„,  v„,  v„)  +MJiv„ 

1  — *_1  /i2—  i_1 

+  — M  ^  CijZj  H - MJ  i  ^  dy-Zj 

^  j=i  ^  j=i 

Thus,  the  linear  systems  in  Eqs.(32)  and  (35)  need  not  be  solved  for  Jx  and  J2.  Finding 
only  the  right  side  of  these  two  linear  systems  suffices  to  compute  the  coefficient  matrix 
II  and  right  side  r*  at  each  integration  stage.  In  [12]  it  is  showed  that  the  matrix 
II  of  Eq.(43)is  the  integration  Jacobian  that  also  appears  in  the  context  of  multistep 
implicit  integration  of  the  DAE  of  multibody  dynamics.  This  observation  allows  for  a 
unitary  implementation  of  implicit  methods,  based  on  either  singly  diagonal  implicit 
Runge-Kutta  formulas  or  on  multistep  BDF-type  formulas. 

The  matrix  II  is  factored  during  Step  6.  The  dimension  of  this  matrix  is  equal 
to  the  number  of  degrees  of  freedom  of  the  mechanical  system  model,  and  is  typically 
small.  Consequently,  here  and  for  that  matter  everywhere  else  the  proposed  algorithm 
uses  LAPACK  and  level  1  and  2  BLAS  dense  linear  algebra  routines  [1] . 

During  Step  7,  the  quantity  M dv/dt  needed  to  compute  the  stage  right  side 
r*  is  evaluated  in  the  consistent  configuration  (u.„,  v„,  u.„,  v„)  from  the  beginning  of 
each  macro-step.  As  the  position  kinematic  constraints  in  Eq.(2a)  are  assumed  time 
independent,  M  does  not  depended  on  time.  Therefore,  using  Eq.(9)  and  Eq.(lOb), 

(46)  M^  =  ^Q(t,v„,v„)  =  Q)'  +  HTQ)1 

The  simplifying  assumptions  made  in  computing  M dv/dt  in  Eq.(46)  are  that  the  kine¬ 
matic  constraint  equations  are  time  independent  and  holonomic.  The  first  assumption 
is  to  quantitatively  simplify  the  presentation.  Otherwise,  the  algorithm  would  step 
by  step  follow  the  derivation  for  the  time  independent  case,  with  the  caveat  that 
terms  of  the  form  3>ut,  <&it,  etc.,  would  have  to  be  accounted  for.  As  a  result, 
all  derivatives  would  be  more  complicated.  The  second  assumption  is  made  because 
covering  the  non-holonomic  constraint  case  is  a  qualitatively  different  process,  which 
is  not  targeted  by  this  paper.  For  a  thorough  account  of  the  non-holonomic  scenario, 
the  reader  is  referred  to  [15].  Finally,  note  that  in  the  case  of  scleronomic  mechanical 
systems,  M dv/dt  =  0. 

The  next  four  steps  compute  the  stage  variables  z,  of  Eq.(25f).  During  each  of 
the  four  stages  of  the  Rosenbrock-Nystrom  algorithm,  some  or  all  of  the  following 
steps  are  taken: 

a)  Obtain  consistent  configuration  at  position  and  velocity  levels. 

b)  Compute  stage  generalized  forces  Q; 
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c)  Compute  stage  right  side  r*  as  in  Eq.(46) 

d)  Solve  stage  linear  system  of  Eq.(43)  to  obtain  z* 

With  regard  to  sub-step  (a)according  to  Eq.(25c),  defining  V*  =  vn  +  hai<fn  + 
h  X] j=i  dijZj.  the  stage  dependent  generalized  coordinates  U*  are  the  solution  of  the 
non-linear  system  <i>(LP,V*)  =  0.  The  matrix  4>u,  along  with  its  factorization,  are 
at  the  cornerstone  of  the  algorithm,  and  Eq.(5)  guarantees  that  U*  is  properly  de¬ 
fined.  Likewise,  denoting  the  stage  independent  velocities  as  V*  =  v„  +  aijzji 
the  stage  dependent  velocities  U,  are,  as  in  Eq.(25d),  the  solution  of  the  linear  sys¬ 
tem  4>U(U®,  V*)U*  =  — 4>v(U®,  V*)V*.  Finally,  note  that  the  stage  forces  Q,  = 
Q  (tn  +  aih,  Vj,  are  computed  during  sub-step  (b),  as  in  Eq.(lOb),  using  the 

factorization  of  the  matrix  4>U(U®,  V*)  available  at  the  end  of  sub-step  (a). 

Due  to  the  particular  choice  of  coefficients  defining  the  Rosenbrock-Nystrom  for¬ 
mula  and  the  way  in  which  the  code  was  implemented,  each  of  the  four  stages  has  its 
own  particularities.  Thus, 

•  Stage  1;  i.e.,  Step  8  of  the  algorithm,  marks  the  beginning  of  a  new  integration 
step,  or  equivalently  the  end  of  the  prior  one.  Therefore  the  system  is  in 
an  assembled  configuration  and  sub-step  (a)  above  is  skipped.  During  this 
stage,  the  matrix  II  of  Eq.(43)  is  evaluated,  and  generalized  accelerations 
are  obtained  as  a  by  product  of  the  process.  Thus,  to  obtain  II,  the  matrix 
M  is  computed  and  the  dependent  constraint  sub-Jacobian  4>u  is  factored. 
The  latter  is  then  used  to  obtain  the  matrices  H,  J,  L,  and  N  of  Section  3. 
Notice  that,  due  to  the  choice  of  a  constant  diagonal  element  7  for  the  original 
Rosenbrock  method,  the  matrix  II  is  constant  and  needs  to  be  factored  only 
once.  The  remaining  3  stages  reuse  this  factorization. 

•  Stages  2  and  3  of  the  Rosenbrock-Nystrom  formula  follow  exactly  the  sub¬ 
steps  (a)  through  (d)  outlined  above. 

•  Stage  4;  i.e.,  Step  11,  bypasses  the  stage  generalized  force  computation  of  Qi 
in  Eq.(46),  because  of  the  special  choice  of  formula  coefficients  (see  Eq.(39)). 
Since  no  force  computation  is  required,  there  is  no  need  to  provide  consistent 
position  and  velocity  configurations,  consequently  sub-step  (a)  is  skipped.  It 
remains  to  compute  the  right  side  r4  and,  with  the  coefficient  matrix  already 
factored  to  do  a  forward/backward  substitution,  to  obtain  Z4. 

During  Step  12,  the  position  level  independent  generalized  coordinates  at  time- 
step  n  +  1  are  computed  according  to  Eq.(25a)  as  v„+i  =  v„  +  hirn  +  hJ2i= 1  Mizi- 
Likewise,  according  to  Eq.(25b),  the  velocity  level  independent  generalized  coordinates 
are  computed  as  v„+i  =  v„  +  ^i=i  & izi ■  The  accuracy  of  the  solution  is  verified  using 
a  second  approximation  of  the  solution  at  time  n  +  1.  The  less  accurate  solution  is 
provided  by  the  embedded  order  3  formula  v„+i  =  vn  +  hirn  +  h  faizi  and 
v„+1  =  v„  +  £l=1  bjZi ,  and  it  is  used  in  Eqs.(14)  and  (16)  for  the  purpose  of  step-size 
control. 

Step  13  computes  dependent  position  level  generalized  coordinates  un+i  such 
that  they  satisfy  ||<J>(u„+i,  v„+1)||00  <  tol.  Dependent  velocities  are  obtained  as  the 
solution  of  the  linear  system  4>u(un+i,  v„+1)u„+i  =  -$v(u„+i,  vn+1)vn+1.  Note 
that  at  the  end  of  this  step  a  factorization  of  the  dependent  constraint  sub-Jacobian 
3>u(un+i,  v„+i)  is  available,  since  it  was  used  to  compute  the  dependent  generalized 
positions. 

Along  with  the  factorization  of  the  dependent  constraint  sub-Jacobian,  its  condition 
number  is  monitored  and  it  is  used  during  Step  14  of  the  algorithm  to  check  the 
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partitioning  of  the  generalized  coordinates.  The  current  partitioning  is  reused  as 
long  as  the  condition  number  of  the  current  dependent  constraint  sub-Jacobian  does 
not  exceed  by  25%  the  value  of  the  condition  number  produced  by  the  most  recent 
partitioning.  This  value  was  determined  after  carrying  out  numerical  experiments 
with  different  values. 


5.  Numerical  Experiments.  A  set  of  numerical  experiments  is  first  carried  out 
to  validate  the  proposed  algorithm.  Then  a  comparison  with  an  explicit  integrator  is 
performed  to  assess  the  efficiency  of  the  proposed  algorithm  for  numerical  integration 
of  a  larger  stiff  mechanical  system. 


5.1.  Validation  of  Proposed  Algorithm.  Validation  is  carried  out  using  the 
double  pendulum  mechanism  shown  in  Fig.l.  Stiffness  is  induced  by  means  of  two 
rotational  spring-damper-actuators  (RSDA).  The  masses  of  the  two  pendulums  are 
mi  =  3  and  m2  =  0.3,  the  dimension  of  the  pendulums  are  L\  =  1  and  L2  =  1.5, 
the  stiffness  coefficients  are  ki  =  400  and  ^2  =  S.E5,  and  the  damping  coefficients  are 
Ci  =  15  and  C2  =  5.E4.  All  units  are  SI.  Note  that  the  zero-tension  angles  for  the  two 
RSDA  elements  are  a®  =  37t/2  and  =  0-  In  its  initial  configuration,  the  two  degree 
of  freedom  dynamic  system  has  a  dominant  eigenvalue  with  a  small  imaginary  part 
and  a  real  part  of  the  order  -10E5.  Since  the  two  pendulums  are  connected  through 
two  parallel  revolute  joints  the  problem  is  planar.  In  terms  of  initial  conditions,  the 
centers  of  mass  (CM)  of  bodies  1  and  2  are  located  at  =  1,  y±M  =  0,  and 

xcm  _  3.4488887,  y^M  =  —0.388228.  In  the  initial  configuration,  the  centroidal 
principal  reference  frame  of  body  1  is  parallel  with  the  global  reference  frame,  while 
the  centroidal  principal  reference  frame  of  body  2  is  rotated  with  62  =  237t/12  around 
an  axis  perpendicular  on  the  plane  of  motion.  For  body  1,  =  yfM  =  =  0, 

while  for  body  2,  =  3.8822857,  =  14.4888887,  and  =  10.  Note  that 

all  initial  conditions  are  in  SI  units  and  are  consistent  with  the  kinematic  constraint 
equations  at  position  and  velocity  levels  (Eqs.(2a)  and  (2b)). 

The  first  set  of  numerical  experiments  focuses  on  assessing  the  reliability  of  the 
step  size  control  mechanism.  The  goal  is  to  verify  that  user  imposed  levels  of  absolute 
and  relative  error  are  met  by  the  simulation  results.  For  this,  a  so  called  reference  sim¬ 
ulation  is  first  run  using  a  very  small  constant  integration  step-size.  Other  simulations, 
run  with  different  combinations  of  absolute  and  relative  tolerances,  are  compared  to 
the  reference  simulation  to  find  the  infinity  norm  of  the  error,  the  time  at  which  this 
largest  error  occurred,  and  average  error  per  time  step.  In  this  context,  suppose  that 
n  time  steps  are  taken  during  the  current  simulation  and  that  the  variable  whose 
accuracy  is  analyzed  is  denoted  by  e.  The  grid  points  of  the  current  simulation  are 
denoted  by  tinu  =  ti  <  t2  <  ■  ■  ■  <  tn  =  If  N  is  the  number  of  time  steps  taken 
during  the  reference  simulation ;  i.e.,  Tinn  =  Xj  <  T2  <  . . .  <  Tn  =  Tencj,  assume 
that  for  the  quantity  of  interest  the  computed  reference  values  are  Ej ,  for  1  <  j  <  N. 
For  each  1  <  i  <  n,  an  integer  r(i)  is  defined  such  that  Tr(q  <  ti  <  Tr(i)+1.  Using 
the  reference  values  Erw-i,  Er (i),  Er(i)+1,  and  Er^+2,  spline  cubic  interpolation  is 
used  to  generate  an  interpolated  value  E*  at  time  f*.  If  r (i)  —  1  <  0,  the  first  four 
reference  points  are  considered  for  interpolation,  while  if  r(i)  +  2  >  N,  the  last  four 
reference  points  are  used  for  interpolation.  The  error  at  time  step  i  is  then  defined  as 
A i  =|  E*  —  e*|.  For  each  tolerance  set  k,  accuracy  is  measured  by  both  the  maximum 

(A^D)  and  the  average  (A  )  trajectory  errors,  as  well  as  by  the  percentage  relative 
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Fig.  1.  Double  pendulum  problem 


error 


=  max  (Aj)  , 


a'*1 


Em 


A(fc) 

RelErr[%]  =  — —  x  100  , 


Simulations  are  run  for  tolerances  between  10-2  and  10— 5 ,  a  range  that  typically 
covers  mechanical  engineering  accuracy  requirements.  The  length  of  the  simulation 
is  2  seconds.  The  time  variation  of  the  angle  9\  is  presented  on  the  left  of  Fig. 2. 
Notice  that  body  1  eventually  stabilizes  in  the  configuration  9\  =  37t/2,  which  is  the 
zero-tension  angle  for  the  RSDA. 

Table  1  contains  error  analysis  information  for  angle  9\ .  The  first  column  contains 
the  value  of  the  tolerance  with  which  the  simulation  is  run.  Relative  and  absolute 
tolerances  ( Rtoli  and  Atoli  of  Eqs.(13))  are  set  to  10fc,  and  they  are  applied  for 
both  position  and  velocity  error  control.  The  second  column  contains  the  time  t* 
at  which  the  largest  error  A(fc)  occurred.  The  third  column  contains  the  values  of 
A^k\  Column  four  contains  the  relative  error,  and  the  last  column  shows  the  average 
trajectory  error. 

The  most  relevant  information  for  step-size  control  validation  is  A^k\  If,  for 
example,  k  =  —3;  i.e.,  accuracy  of  the  order  10-3  is  demanded,  A(_3)  should  have  this 
order  of  magnitude.  It  can  be  seen  from  the  results  in  Table  1  that  this  is  the  case  for 
all  tolerances.  Notice  that  these  results  are  obtained  with  a  non-zero  relative  tolerance. 
According  to  Eq.(13),  depending  on  the  magnitude  of  the  variable  being  analyzed,  the 
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Fig.  2.  Time  Variation  of  orientation  6\  (Left)  and  of  angular  velocity  8\  (Right)  for  Body  1. 


relative  tolerance  loosens  or  tightens  the  step-size  control.  Based  on  results  shown 
on  the  left  of  Fig. 2,  the  relative  tolerance  is  multiplied  by  a  value  that  oscillates 
between  4.0  and  6.0.  Consequently,  the  actual  upper  bound  of  accuracy  imposed  on 
fluctuates  and  reaches  values  up  to  7-10-*.  Thus,  the  step-size  controller  is  slightly 
conservative.  For  an  explanation  of  this  stiffness  induced  order  reduction,  the  reader 
is  referred  to  [7].  To  remedy  this,  in  [18],  a  scaling  of  the  truncation  error  that  enters 
in  Eq.(13)  the  computation  of  the  new  step-size  is  recommended.  In  this  context, 
the  quantity  (y±  —  y\)  is  replaced  by  the  scaled  value  8  =  (I  —  hjdf  /dy)-1  (iji  —  yi). 
This  step-size  control  strategy  remains  to  be  investigated. 

Table  1 

Position  Error  Analysis  for  the  Double  Pendulum  Problem. 


k 

t* 

A« 

RelErr [%] 

AW 

-2 

0.592127 

5 . 223e-2 

0.12126 

3 . 234e-3 

-3 

0.599954 

4. 198e-3 

0.00964 

2 . 631e-4 

-4 

0.626135 

4. 916e-4 

0.00108 

2.946e-5 

-5 

1.065146 

1.902e-5 

0.00039 

9 . 868e-6 

Table  2 

Velocity  Error  Analysis  for  the  Double  Pendulum  Problem. 


k 

t* 

A (fc) 

RelErr  [°/J 

A(fc) 

-2 

0.795548 

4. 061e-2 

1 . 84434 

2 . 348e-2 

-3 

0.373114 

3 . 792e-3 

0.12340 

2.181e-3 

-4 

0.217757 

8 . 652e-4 

0.00922 

3.445e-4 

-5 

0.186183 

2 . 343e-4 

0.00246 

9.357e-5 

Error  analysis  is  also  performed  at  the  velocity  level.  The  time  variation  of  angular 
velocity  9i  is  shown  on  the  right  of  Fig. 2.  The  angular  velocity  of  body  1  fluctuates 
between  -10  and  7  rad/s.  As  a  result,  values  of  A(fc)  of  up  to  the  order  10fc+1  are 
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considered  very  good.  Error  analysis  results  for  61  are  presented  in  Table  2.  The 
step-size  controller  performs  well,  slightly  on  the  conservative  side. 

The  error  analysis  results  presented  suggest  that  the  step-size  controller  employed 
is  reliable.  The  step-size  control  mechanism  used  indicates  that  using  an  embedded 
formula  for  local  error  estimation  is  suitable,  since  for  the  test  problem  considered 
accuracy  requirements  are  met  or  exceeded  by  the  numerical  results.  In  order  to 
avoid  unjustified  CPU  penalties,  the  algorithm  may  be  improved  for  extremely  stiff 
mechanical  systems  by  adopting  the  step-size  controller  proposed  in  [18]. 


5.2.  Performance  Comparison  with  Explicit  Integrator.  In  order  to  com¬ 
pare  the  performance  of  the  proposed  implicit  algorithm  with  a  state  of  the  art  SSODE 
explicit  integrator,  a  model  of  the  US  Army  High  Mobility  Multipurpose  Wheeled  Ve¬ 
hicle  (HMMWV)  is  considered  for  dynamic  analysis.  The  HMMWV  shown  in  Fig. 3  is 
modeled  using  14  bodies,  as  shown  in  Fig.4.  In  this  figure,  vertices  represent  bodies, 
while  edges  represent  joints  connecting  the  bodies  of  the  system.  Thus,  vertex  number 
1  is  the  chassis,  2  and  5  are  the  right  and  left  front  upper  control  arms,  3  and  6  are 
the  right  and  left  front  lower  control  arms,  9  and  12  are  the  right  and  left  rear  lower 
control  arms,  and  8  and  11  are  the  right  and  left  rear  upper  control  arms.  Bodies 
4,  7,  10,  and  13  are  the  wheel  spindles,  and  body  14  is  the  steering  rack.  Spherical 
joints  are  denoted  by  S,  revolute  joints  by  R,  distance  constraints  by  D,  and  transla¬ 
tional  joints  by  T.  This  set  of  joints  imposes  79  constraint  equations.  One  additional 
constraint  equation  is  imposed  on  the  steering  system,  such  that  the  steering  angle  is 
zero;  i.e.,  the  vehicle  drives  straight.  A  total  of  98  generalized  coordinates  are  used 
to  model  the  vehicle,  which  renders  18  degrees  of  freedom  to  the  model. 

Stiffness  is  induced  in  the  model  through  means  of  four  translational  spring- 
damper  actuators  (TSDA).  These  TSDAs  act  between  the  front/rear  and  right /left 
upper  control  arms  and  the  chassis.  The  stiffness  coefficient  of  each  TSDA  is  2E07 
N/m,  while  the  damping  coefficient  is  2E06N  ■  s/m.  For  the  purpose  of  this  numerical 
experiment,  the  tires  of  the  vehicle  are  modeled  as  vertical  TSDA  elements  with 
stiffness  coefficient  296325  N/m  and  damping  coefficient  3502V  ■  s/m.  Finally,  the 
dominant  eigenvalue  of  the  corresponding  SSODE  of  Eq.(9)  has  a  real  component  of 
approximately  — 2.6E5,  and  a  small  imaginary  part. 

Dynamic  analysis  of  the  model  is  carried  out  for  the  vehicle  driving  straight  at 
lOmph  over  a  bump.  The  shape  of  the  bump  is  a  half-cylinder  of  diameter  0.1m. 
Figure  4  shows  the  time  variation  of  the  vehicle  chassis  height.  The  front  wheels  hit 
the  bump  at  time  0.5  seconds,  and  the  rear  wheels  hit  the  bump  at  time  1.2  seconds. 
The  length  of  the  simulation  in  this  plot  is  5  seconds.  Toward  the  end  of  the  simulation 
(after  4  seconds),  due  to  over-damping  the,  chassis  height  stabilizes  at  approximately 
Zi  =  0.71m. 


The  test  problem  is  first  run  with  an  explicit  integrator  based  on  the  code  DEABM 
of  Shampine  and  Watts  [18].  Algorithm  2  outlines  the  explicit  integration  approach 
used  for  SSODE  integration  of  the  equations  of  motion  for  the  HMMWV  model. 


Rosenbrock-Nystrom  Methods  for  Multibody  Dynamics 


21 


Fig.  3.  HMMWV 


Fig.  4.  HMMWV  model  representation 
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Algorithm  2 

1.  Initialize  Simulation 

2.  Set  Integration  Tolerance 

3.  While  (time  <  time-end)  do 

4.  Get  Acceleration 

5.  Apply  Integration  Step. 

Check  Accuracy.  Determine  New  Step-size 

6.  Recover  Dependent  Generalized  Coordinates 

7.  Check  Partition 

8.  End  do 

The  first  3  steps  are  identical  to  the  ones  in  Algorithm  1.  Step  4  computes  the 
acceleration  q,  by  solving  the  linear  system  of  Eq.(3).  A  topology-based  approach 
[17],  that  takes  into  account  the  sparsity  of  the  coefficient  matrix  is  used  to  solve  for 
the  generalized  accelerations  q.  The  DDEABM  integrator  of  Shampine  and  Watts  is 
then  used  to  integrate  for  independent  velocities  v„,  and  independent  positions  v„. 
The  integrator  is  also  used  to  integrate  for  the  dependent  coordinates  u „,  with  the  sole 
purpose  of  providing  a  good  starting  point  during  Step  6  for  the  iterative  solver.  At 
each  time  step  tn  it  computes  u„  by  ensuring  that  the  kinematic  position  constraint 
equations  are  satisfied;  i.e.,  solving  4>(vn,u„)  =  0.  Likewise,  dependent  velocities 
ii„  are  the  solution  of  the  linear  system  4>u(u„,  v„)u„  =  -§v(un,vn)v„,  which  thus 
guarantees  that  the  generalized  velocities  satisfy  the  kinematic  velocity  constraint 
equations.  The  dependent /independent  partitioning  of  the  generalized  coordinates  is 
checked  during  Step  7  and  with  this  the  algorithm  concludes  one  integration  step  and 
proceeds  to  the  next  one. 

Timing  results  reported  are  obtained  on  a  SGI  Onyx  computer  with  an  R10000 
processor.  Computer  times  required  by  Algorithm  2  are  listed  in  Table  3.  Results 
for  the  Rosenbrock  Nystrom  algorithm  are  presented  in  Table  4. 

Table  3 

Explicit  Integrator  Timing  Results  for  the  HMMWV  Problem. 


Tol 

m 

n 

eebm 

n 

1  sec 

3618 

3641 

3667 

3663 

2  sec 

7276 

7348 

7287 

7276 

3  sec 

10865 

11122 

10949 

10965 

4  sec 

14480 

14771 

14630 

14592 

Table  4 

Implicit  Integrator  Timing  Results  for  the  HMMWV  Problem. 


Tol 

10-2 

10-3 

10-4 

10-6 

1  sec 

5.6 

13.2 

40.7 

172 

2  sec 

12.6 

32.6 

95 

405 

3  sec 

13 

36.3 

105 

422 

4  sec 

13.3 

37 

106 

428 

Results  in  Table  3  are  typical  for  the  situation  in  which  an  explicit  integrator  is 
used  for  the  numerical  solution  of  a  stiff  IVP.  For  the  stiff  test  problem  considered, 
the  performance  limiting  factor  is  stability  of  the  explicit  code.  For  any  tolerance  in 
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the  range  IE-2  through  IE-5  and  any  given  simulation  length,  CPU  times  are  almost 
identical.  The  average  explicit  integration  step-size  turns  out  to  be  between  IE-5 
and  IE-6,  and  it  is  not  affected  by  accuracy  requirements.  The  code  is  compelled 
to  select  very  small  step-sizes  to  assure  stability  of  the  integration  process,  and  this 
is  the  criteria  for  step-size  selection  for  a  broad  spectrum  of  tolerances.  Only  when 
extremely  sever  accuracy  constraints  are  imposed  on  integration,  does  the  step-size 
become  limited  by  accuracy  considerations.  In  this  context,  note  that  the  results 
in  Table  4  indicate  that  stability  is  of  no  concern  for  the  proposed  algorithm,  and 
solution  accuracy  solely  determines  the  duration  of  the  simulation.  As  expected,  more 
accurate  results  demand  longer  CPU  times.  In  this  situation,  the  integration  step- 
size  is  automatically  adjusted  to  keep  integration  error  within  the  prescribed  limits. 
Figure  5  shows  the  time  variation  for  the  integration  step-size  when  the  absolute  and 
relative  errors  at  position  and  velocity  levels  are  set  to  10-3.  The  y- axis  for  the  step- 
size  is  provided  at  the  right  of  Fig. 5,  on  a  logarithmic  scale.  In  the  lower  half  of  the 
same  figure,  relative  to  the  left  y-axis  is  provided  the  time  variation  of  the  chassis 
height.  Note  that  when  the  vehicle  hits  the  bump;  i.e.,  when  in  Fig. 5  the  z  coordinate 
of  the  chassis  increases  suddenly,  the  step-size  is  simultaneously  decreased  to  preserve 
accuracy  of  the  numerical  solution.  On  the  other  hand,  for  the  region  in  which  the 
road  becomes  flat;  i.e.,  toward  the  end  of  the  simulation,  the  integrator  is  capable  of 
taking  larger  integration  steps,  thus  decreasing  simulation  time. 


Chassis  Height  and  Integration  Step-Size 


Simulation  time  [seconds] 


Fig.  5.  Chassis  height  and  integration  step-size 


6.  Conclusions.  A  generalized  coordinate  partitioning  based  state-space  im¬ 
plicit  integration  method  is  presented  for  dynamic  analysis  of  multibody  systems. 
The  method  is  based  on  a  Rosenbrock  type  formula  with  4  stages  that  does  not  re¬ 
quire  the  solution  of  a  non-linear  system  for  the  stage  values.  The  order  4  formula  is 
L-stable  and  has  an  embedded  order  3  formula  for  error  control.  For  a  14  body  18 
degree  of  freedom  vehicle,  the  proposed  algorithm  is  almost  two  orders  of  magnitude 
faster  than  an  explicit  integrator  based  method.  There  is  room  for  improvement  as  far 
as  the  step-size  controller  is  concerned,  as  it  is  seen  to  be  conservative  when  very  stiff 
models  are  run  under  stringent  accuracy  requirements.  The  most  restrictive  condition 
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imposed  on  the  user  by  the  Rosenbrock  formula  employed  is  the  requirement  of  an 
exact  integration  Jacobian.  A  formalism  is  presented  for  computing  the  analytical 
integration  Jacobian  required.  When  providing  an  exact  Jacobian  is  not  possible,  a 
lower  order  W-method  is  suggested  as  an  alternative. 
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